#include "../common/irobot_abstract2.h"
#include "../common/irobot_abstract2.h"
#include "../common/oi.h"

#include <limits.h>
#ifdef SERIAL
#include <math.h>
#endif

#define bound_velocity(x) (x <= 500 ? x : (x >= 0 ? x : -500))


//create office theme song as song number 0
void create_office_theme_song(void)
{
	ir_send_byte(CmdSong);
	ir_send_byte(0);
	ir_send_byte(14);
	ir_send_byte(67);
	ir_send_byte(32);
	ir_send_byte(79);
	ir_send_byte(64);
	ir_send_byte(78);
	ir_send_byte(8);
	ir_send_byte(79);
	ir_send_byte(8);
	ir_send_byte(78);
	ir_send_byte(8);
	ir_send_byte(74);
	ir_send_byte(8);
	ir_send_byte(76);
	ir_send_byte(64);
	ir_send_byte(72);
	ir_send_byte(16);
	ir_send_byte(72);
	ir_send_byte(16);
	ir_send_byte(72);
	ir_send_byte(8);
	ir_send_byte(71);
	ir_send_byte(8);
	ir_send_byte(69);
	ir_send_byte(8);
	ir_send_byte(71);
	ir_send_byte(8);
	ir_send_byte(67);
	ir_send_byte(64);
}

//play office theme song
void play_office_theme_song(void)
{
	ir_send_byte(CmdPlay);
	ir_send_byte(0);
}

#define dist(x1,y1,x2,y2) sqrt(pow((double)x1-(double)x2,2)+pow((double)y1-(double)y2,2))
int main(int argc, char **argv) {
	ir_initialize(0);
	ir_sleep(100);
	
	ir_send_byte(CmdStart);
	ir_send_byte(CmdFull);

/// light the leds--idiot test
	ir_sleep(100);
	leds(false,false,0,255);

	create_office_theme_song();

	int x = 0;
	int y = 0;
	do {
		collect_sensor_data();
		drive(150,150);
		y += distance();
		printf("y = %d\n", y);
	} while(y < 1000);

	stop(); ir_sleep(1000);

	int _angle = 0;
	do {
		collect_sensor_data();
		drive(150,-150);
		_angle += angle();
		printf("a=%d\n",_angle);
	} while(_angle < 90);

	stop(); ir_sleep(1000);

	do {
		collect_sensor_data();
		drive(200,200);
		x += distance();
	} while(x < 1000);

	stop(); ir_sleep(1000);

	_angle = 0;
	do {
		collect_sensor_data();
		drive(150,-150);
		_angle += angle();
	} while(_angle < 90);

	stop(); ir_sleep(1000);

	x=0; y=0; // relocalize
	int card_index = 0;
	int card_x[3] = {UINT_MAX,UINT_MAX,UINT_MAX};
	int card_y[3] = {UINT_MAX,UINT_MAX,UINT_MAX};


	bool side = false;
	bool down = true;
	int tmp_dist = 0;
	while(x < 2000) {
		collect_sensor_data();

		if(left_bumper() || right_bumper()){
			stop();
			leds(0,0,255,255);
			ir_finish();
			exit(0);
		}

		// update location
		tmp_dist += distance();
		if(side) { x += distance(); }
		else {
			if(down) y += distance();
			else y -= distance();
		}


		// drive mower shape
		printf("x = %4d,\ty = %4d,\td = %4d,\tdown = %d,\tside = %d\n", x,y,tmp_dist,down,side);
		if(tmp_dist < (side?350:2000)){
			drive(150,150);
		} else {
			stop();
			ir_sleep(1000);
			_angle = 0;
			do {
				collect_sensor_data();
				if(down) drive(150,-150);
				else drive(-150,150);
				_angle += angle();
				printf("a = %3d\n",_angle);
			} while(_angle < 90 &&  _angle > -90);
			if(side) down = !down;
			side = !side;
			tmp_dist = 0;
			stop();
			ir_sleep(1000);
		}

		// check cliff sensors
		printf("l: %d\tfl: %d\tfr: %d\tr: %d\n", cliff_left_signal(),
			cliff_front_left_signal(),cliff_front_right_signal(),cliff_right_signal());
		if(cliff_left_signal() > 1000 || cliff_right_signal() > 1000 ||
				cliff_front_left_signal() > 1000 || cliff_front_right_signal() > 1000){
			bool new = true;
			int _x,_y, d;
			d = (down?1:-1);
			// guess where the card is based on where it is with respect to the robot
			if(cliff_left_signal() > 1000 && cliff_front_left_signal() < 1000){
				_x = x+130*d;
				_y = y+ 40*d;
			} else if(cliff_left_signal() > 1000 && cliff_front_left_signal() > 1000){
				_x = x+ 60*d;
				_y = y+ 70*d;
			} else if(cliff_front_left_signal() > 1000 && cliff_front_right_signal() < 1000){
				_x = x+ 20*d;
				_y = y+130*d;
			} else if(cliff_front_left_signal() > 1000 && cliff_front_right_signal() > 1000){
				_x = x;
				_y = y+130*d;
			} else if(cliff_front_left_signal() < 1000 && cliff_front_right_signal() > 1000){
				_x = x- 20*d;
				_y = y+130*d;
			} else if(cliff_right_signal() > 1000 && cliff_front_right_signal() > 1000){
				_x = x- 60*d;
				_y = y+ 70*d;
			} else if(cliff_right_signal() > 1000 && cliff_front_right_signal() < 1000){
				_x = x-130*d;
				_y = y+ 40*d;
			} else {
				_x = x;
				_y = y;
			}



			//check if it is not with in a 300 mm radius of any prev. found cards
			printf("NEW? _x = %4d,\t_y = %4d\n", _x,_y);
			int i;for(i = 0; i < card_index; i++){
				printf("NEW? x%d = %4d,\ty%d = %4d\n",i,card_x[i],i,card_y[i]);
				printf("NEW? dist = %f\n",dist(_x,_y,card_x[i],card_y[i]));
				new = new && dist(_x,_y,card_x[i],card_y[i]) > 300;
			}

			if(new){
				play_office_theme_song();
				stop();ir_sleep(2000);			
				card_x[card_index] = _x;
				card_y[card_index] = _y;
				++card_index;
				leds(1,1,255,255);
		} else leds(1,1,0,0);
			if(card_index == 3) break;
		} else {
			leds(0,0,0,0);
		}
	}

	stop(); ir_sleep(1000);



	//find direction to go to first card
	/*
	double theta;
	theta = atan2((double)(y-card_y[0]),(double)(x-card_x[0]));
	theta *= 57.2957795;

	if(down && theta >= 0) theta = 180.0 + theta;
	else if(!down && theta <= 0) theta = 180.0 + theta;

	printf("theta = %d\n",theta);
	_angle = 0;
	do { // turn
		collect_sensor_data();
		drive(150,-150);
		_angle += angle();
		printf("a = %3d\n",_angle);
	} while(_angle < theta && _angle > -theta);
	stop(); ir_sleep(1000);

	tmp_dist = 0;
	do { // drive towards first card
		collect_sensor_data();
		drive(150,150);
		tmp_dist += distance();
	} while(tmp_dist < dist(x,y,card_x[0],card_y[0]));
	stop();ir_sleep(1000);
	*/

	//move to correct orientation
	
	if(side){
		if(y > card_y[0]){
			_angle = 0;
			do {//turn left
				collect_sensor_data();
				drive(150,-150);
				_angle += angle();
			} while(_angle < 90 &&  _angle > -90);
			stop();ir_sleep(1000);

			tmp_dist = 0;
			do { // drive the y
				collect_sensor_data();
				drive(150,150);
				tmp_dist += distance();
			} while(tmp_dist < y - card_y[0]);			
			stop();ir_sleep(1000);

			_angle = 0;		
			do {//turn left
				collect_sensor_data();
				drive(150,-150);
				_angle += angle();
			} while(_angle < 90 &&  _angle > -90);
			stop();ir_sleep(1000);
			tmp_dist = 0;
			do { // drive the x
				collect_sensor_data();
				drive(150,150);
				tmp_dist += distance();
			} while(tmp_dist < x - card_x[0]);		
			stop();ir_sleep(1000);
		}
		else{
			_angle = 0;
			do {//turn right
				collect_sensor_data();
				drive(-150,150);
				_angle += angle();
			} while(_angle < 90 &&  _angle > -90);
			stop();ir_sleep(1000);
			tmp_dist = 0;		
			do { // drive the y
				collect_sensor_data();
				drive(150,150);
				tmp_dist += distance();
			} while(tmp_dist < card_y[0] - y);			
			stop();ir_sleep(1000);

			_angle = 0;		
			do {//turn right
				collect_sensor_data();
				drive(-150,150);
				_angle += angle();
			} while(_angle < 90 &&  _angle > -90);
			stop();ir_sleep(1000);
			tmp_dist = 0;
			do { // drive the x
				collect_sensor_data();
				drive(150,150);
				tmp_dist += distance();
			} while(tmp_dist < x - card_x[0]);
			stop();ir_sleep(1000);
		}
	}
	else{
		if(down){
			if(y > card_y[0]){
				_angle = 0;
				do {//turn right
					collect_sensor_data();
					drive(-150,150);
					_angle += angle();
				} while(_angle < 90 &&  _angle > -90);
				stop();ir_sleep(1000);
				tmp_dist = 0;			
				do { // drive the x
					collect_sensor_data();
					drive(150,150);
					tmp_dist += distance();
				} while(tmp_dist < x - card_x[0]);			
				stop();ir_sleep(1000);

				_angle = 0;		
				do {//turn right
					collect_sensor_data();
					drive(-150,150);
					_angle += angle();
				} while(_angle < 90 &&  _angle > -90);
				stop();ir_sleep(1000);
				tmp_dist = 0;			
				do { // drive the y
					collect_sensor_data();
					drive(150,150);
					tmp_dist += distance();
				} while(tmp_dist < y - card_y[0]);			
				stop();ir_sleep(1000);
			}
			else{
				_angle = 0;
				do {//turn right
					collect_sensor_data();
					drive(-150,150);
					_angle += angle();
				} while(_angle < 90 &&  _angle > -90);
				stop();ir_sleep(1000);
				tmp_dist = 0;						
				do { // drive the x
					collect_sensor_data();
					drive(150,150);
					tmp_dist += distance();
				} while(tmp_dist < x - card_x[0]);	
				stop();ir_sleep(1000);

				_angle = 0;		
				do {//turn left
					collect_sensor_data();
					drive(150,-150);
					_angle += angle();
				} while(_angle < 90 &&  _angle > -90);
				stop();ir_sleep(1000);
				tmp_dist = 0;			
				do { // drive the y
					collect_sensor_data();
					drive(150,150);
					tmp_dist += distance();
				} while(tmp_dist < card_y[0] - y);			
				stop();ir_sleep(1000);
			}
		}
		else{
			if(y > card_y[0]){
				_angle = 0;
				do {//turn left
					collect_sensor_data();
					drive(150,-150);
					_angle += angle();
				} while(_angle < 90 &&  _angle > -90);
				stop();ir_sleep(1000);
				tmp_dist = 0;						
				do { // drive the x
					collect_sensor_data();
					drive(150,150);
					tmp_dist += distance();
				} while(tmp_dist < x - card_x[0]);			
				stop();ir_sleep(1000);

				_angle = 0;		
				do {//turn right
					collect_sensor_data();
					drive(-150,150);
					_angle += angle();
				} while(_angle < 90 &&  _angle > -90);
				stop();ir_sleep(1000);
				tmp_dist = 0;			
				do { // drive the y
					collect_sensor_data();
					drive(150,150);
					tmp_dist += distance();
				} while(tmp_dist < y - card_y[0]);			
				stop();ir_sleep(1000);
			}
			else{
				_angle = 0;
				do {//turn left
					collect_sensor_data();
					drive(150,-150);
					_angle += angle();
				} while(_angle < 90 &&  _angle > -90);
				stop();ir_sleep(1000);
				tmp_dist = 0;						
				do { // drive the x
					collect_sensor_data();
					drive(150,150);
					tmp_dist += distance();
				} while(tmp_dist < x - card_x[0]);	
				stop();ir_sleep(1000);

				_angle = 0;		
				do {//turn left
					collect_sensor_data();
					drive(150,-150);
					_angle += angle();
				} while(_angle < 90 &&  _angle > -90);
				stop();ir_sleep(1000);
				tmp_dist = 0;			
				do { // drive the y
					collect_sensor_data();
					drive(150,150);
					tmp_dist += distance();
				} while(tmp_dist < card_y[0] - y);			
				stop();ir_sleep(1000);
			}
		}
	}

	_angle = 0;
	do{ // do an archemedian spiral till a card is seen
		collect_sensor_data();
		turn_about(250,20*0.0174532925*(_angle += angle()));
	} while(cliff_left_signal() < 1000 && cliff_right_signal() < 1000 &&
				cliff_front_left_signal() < 1000 && cliff_front_right_signal() < 1000);

	stop();ir_sleep(1000);

	_angle = 0;
	while(_angle < 360){ // get directly ontop of the card
		collect_sensor_data();
		if(cliff_right_signal() > 1000 || cliff_front_left_signal() > 1000){
			drive(75,75);
			_angle = 0;
			ir_sleep(100);
		} else {
			_angle += angle();
			drive(100,-100);
		}
		printf("a = %3d\n",_angle);
	}
	stop();
	leds(0,0,0,255);
	//play_office_theme_song();
	ir_finish();

}
